// SEE END OF FILE FOR LICENSE TERMS

#include "libSENS.h"
#include "barometricAltitude.h"

#include <math.h>
//#include <dsp.h>


// The offset in altitude between absolute and relative
unsigned int altitudeOffset = 0;

// absolute altitude vs sea level (it can be negative)
// +-0.01m
// Range = +-21.4km
unsigned int	absoluteAltitude = 0;

// relative adjusted altitude vs zero offset point
// +-0.01m
// Range = +-21.4km
unsigned int	relativeAltitude = 0;

// Ground altitude that corresponds to ground pressure
unsigned int groundAltitude;


// Baromentric pressure
// 	LSB = 1uBar = 0.001mbar = 0.1pascal
// 	Full range is 4294.7bar
unsigned long barometricPressure = 1000000;		// 1000mbar default

// Ground barometric pressure, used to correct for atmospheric pressure changes
// 	LSB = 1uBar = 0.001mbar = 0.1pascal
// 	Full range is 4294.7bar
unsigned long groundPressure	= STANDARD_SEA_LEVEL_PRESSURE;



// Pressure at sea level in pascal		// THIS MUST VARY WITH GROUND PRESSURE
#define pressure_sea_level 101325.0f


void updateAltitude(void)
{
	float pressure_ambient = (float) (barometricPressure * 10);

	absoluteAltitude = (unsigned int) (44330.0f * ((1-pow((pressure_ambient/pressure_sea_level),(1/5.255f)))) ); // Meters
	
	relativeAltitude = absoluteAltitude + altitudeOffset;
};


void setAltitudeOffset(unsigned int altitude)
{
};


void setGroundPressure(unsigned int altitude, unsigned long pressure)
{
};

/****************************************************************************/
// This is part of the servo and radio interface software
//
// ServoInterface source code
//	http://code.google.com/p/rc-servo-interface
//
// Copyright 2010 ServoInterface Team
// See the AUTHORS.TXT file for a list of authors of ServoInterface.
//
// ServoInterface is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// ServoInterface is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License 
// along with ServoInterface.  If not, see <http://www.gnu.org/licenses/>.
//
// Many parts of ServoInterface use either modified or unmodified code
// from the MatrixPilot pilot project.
// The project also contains code for modifying MatrixPilot to operate
// with ServoInterface.
// For details, credits and licenses of MatrixPilot see the AUTHORS.TXT file.
// or see this website: http://code.google.com/p/gentlenav
/****************************************************************************/
